#ifndef SETROBOT_HPP
#define	SETROBOT_HPP

#include "visualization.hpp"
#include "robot.hpp"

#include <opencv/cv.h>
#include <opencv/highgui.h>

#include <cstdio>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_eigen.h>

using namespace cv;

class SetRobot {
public:

    SetRobot() {

        this->img = cvCreateImage(cvSize(640, 480), IPL_DEPTH_8U, 3);

              //  H = gsl_matrix_alloc(3, 4);
              /*  double h1, h2, h3, h4;

                this->h_file = fopen("homography.txt", "r");
        
                fscanf(h_file, "%lf %lf %lf %lf", &h1, &h2, &h3, &h4);
                gsl_matrix_set(H, 0, 0, h1);
                gsl_matrix_set(H, 0, 1, h2);
                gsl_matrix_set(H, 0, 2, h3);
                gsl_matrix_set(H, 0, 3, h4);
                fscanf(h_file, "%lf %lf %lf %lf", &h1, &h2, &h3, &h4);
                gsl_matrix_set(H, 1, 0, h1);
                gsl_matrix_set(H, 1, 1, h2);
                gsl_matrix_set(H, 1, 2, h3);
                gsl_matrix_set(H, 1, 3, h4);
                fscanf(h_file, "%lf %lf %lf %lf", &h1, &h2, &h3, &h4);
                gsl_matrix_set(H, 2, 0, h1);
                gsl_matrix_set(H, 2, 1, h2);
                gsl_matrix_set(H, 2, 2, h3);
                gsl_matrix_set(H, 2, 3, h4);
        */    
    }

    ~SetRobot() {
        cvReleaseImage(&this->img);
       // delete H;
       // fclose(this->h_file);
    }

    void setupKalmanFilterRobots();

    INFO_ROBOT getInfoRobotNUM(int num) {
        return robot[num].getRobotInfo();
    }

    void setInfoRobotNum(int num, INFO_ROBOT info) {
        robot[num].setRobotInfo(info);
    }

    void setSetRobotPosition(int num, INFO_ROBOT info) {

        CvPoint center1, center2;
        center1.x = info.pixelX1;
        center1.y = info.pixelY1;
        center2.x = info.pixelX2;
        center2.y = info.pixelY2;

        this->setPosition(num, center1, center2);
    }

    bool findRobotsPose2D(int __numRect, int *__colors, CvPoint *__centers);

    void printInformationSetRobo();

    IplImage* showKalmanFilterAction() {

        int i;
        CvPoint center1, center2;
        INFO_ROBOT info;
        for (i = 0; i < NUM_MAX_ROBOTS; i++) {
            info = robot[i].getRobotInfo();

            center1.x = info.pixelX1;
            center1.y = info.pixelY1;
            center2.x = info.pixelX2;
            center2.y = info.pixelY2;

            positionDrawImage1[i].push_back(center1);
            positionDrawImage2[i].push_back(center2);
        }

        cvZero(img);

        for (i = 0; i < positionDrawImage1[0].size() - 1; i++) {
            cvLine(img, positionDrawImage1[0][i], positionDrawImage1[0][i + 1], Scalar(255, 0, 0), 1);
            cvLine(img, positionDrawImage2[0][i], positionDrawImage2[0][i + 1], Scalar(0, 255, 0), 1);
        }

        return img;
    }

private:

    // Given the image coordinate of a point (u,v), the homography matrix M, and the component Z (zpos) of the world coordinate of a point, 
    // this function computes the coordinate X (xpos) and Y (ypos) of the point
    void image2world(int u, int v, double zpos, double *xpos, double *ypos);

    void setPosition(int __numRobo, CvPoint __centers1, CvPoint __centers2);
    void setPositionPixel(int __numRobo, CvPoint __centers1, CvPoint __centers2);

    /*
     * O codigo esta trabalhando com as seguintes condicoes, de posicao de robos
     * A flecha indica o sentido de orientacao do robo, com relacao ao eixo x 
     * base
     * 0 Cyan -> Green   
     * 1 Cyan -> Pink
     * 2 Cyan -> Orange
     * 3 Pink -> Green
     * 4 Pink -> Orange
     * 5 Green -> Orange  
     */
    FILE* h_file;
    IplImage *img;
    unsigned int numRobots;
    gsl_matrix *H;
    int nLeftoffset;
    int nTopoffset;
    Robot robot[NUM_MAX_ROBOTS];
    bool allLocalizate, ok;
    std::vector<CvPoint> positionDrawImage1[NUM_MAX_ROBOTS];
    std::vector<CvPoint> positionDrawImage2[NUM_MAX_ROBOTS];
};

#endif	/* SETROBOT_HPP */

